#include <stdlib.h>
#include <stdio.h>
#include <fcntl.h>    /* For O_RDWR */
#include <unistd.h>   /* For open(), creat() */
#include <string.h>
#include <termios.h>
#include <errno.h>

double heading, pitch, roll, temp;
int fd;
bool goodData;

int compass_connect(){
	heading = 0;
	pitch = 0;
	roll = 0;
	temp = 0;

	fd = open("/dev/os5000", O_RDONLY | O_NOCTTY | O_NONBLOCK);// | O_NDELAY);
	fprintf(stderr, "Opening compass..%d\n", fd);
	if (fd == -1)
	{
		fprintf(stderr, "error opening Compass\n");
		return -1;
	}

	struct termios options;
	//if (cfsetispeed(&options, B19200) < 0) { fprintf(stderr, "error setting baud\n"); return -1; } // set baud rates
	if (cfsetspeed(&options, B57600) < 0) { fprintf(stderr, "error setting baud\n"); return -1; } // set baud rates
	options.c_cflag |= (CLOCAL | CREAD);
	options.c_cflag &= ~PARENB;
	options.c_cflag &= ~CSTOPB;
	options.c_cflag &= ~CSIZE;
	options.c_cflag |= CS8;

	if (tcsetattr(fd, TCSANOW, &options) < 0){ fprintf(stderr, "error setting options\n"); return -1; } 
	fprintf(stderr, "Compass opened! %d\n", fd);

	char n;
	fd_set rfds;
	struct timeval timeout;
	
	// initialise the timeout structure
	timeout.tv_sec = 3; // 3 second timeout
	timeout.tv_usec = 0;
	
	FD_ZERO( &rfds );
	FD_SET( fd, &rfds );
	// do the select
	n = select(fd + 1, &rfds, NULL, NULL, &timeout);
	
	// check if an error has occured
	if(n < 0)
	{
		fprintf(stderr, "Select failed\n");
		exit(1);	
	}
	else if (n == 0)
	{
	 	fprintf(stderr, "Timeout!");
		exit(1);
	}
	else
	{
		fprintf(stderr, "Bytes detected on compass port, success\n");
	}
	return fd;
}

int compass_disconnect()
{
	close(fd);
	return 0;
}


int read_compass()
{
	//fprintf(stderr, "Reading compass %d\n", fd);
	char buf[256] = "";
	//char n;
	fd_set rfds;
	struct timeval timeout;
	
	// initialise the timeout structure
	timeout.tv_sec = 2; // 1 second timeout
	timeout.tv_usec = 0;
	
	FD_ZERO( &rfds );
	FD_SET( fd, &rfds );
	// do the select
	select(fd + 1, &rfds, NULL, NULL, &timeout);
	
	int bytes_read = read(fd,buf,250);


	while (bytes_read == -1)
	{
		fprintf(stderr, "%d error, %s\n", errno, strerror(errno));
		bytes_read = read(fd,buf,250);	
		//fprintf(stderr, "%d bytes read \n", bytes_read);
		usleep(50000);	
	}
	
	//fprintf(stderr, "%d bytes read \n", bytes_read);
	//fprintf(stderr, "%s\n", buf);

	if (bytes_read >= 25)
	{
		// Code below from S. Stancliff
		char *pS;
		int rval;
		pS = strrchr(buf,'$');
		while(pS == NULL){
			fprintf(stderr, "parse_compass(): No '$' found: %s\n", buf);
			compass_disconnect();
			compass_connect();
			return -1;
		}

		rval = sscanf(pS
					,"$C%lfP%lfR%lfT%lf*"
					,&(heading)
					,&(pitch)
					,&(roll)
					,&(temp)
					);
				
		if(rval == EOF){
			fprintf(stderr, "parse_compass(): Incomplete string\n");
			return -1;
		}
	}
	else
	{
		return -1;
	}
	//fprintf(stderr, "Done reading compass: %lf, %lf, %lf, %lf\n", heading, pitch, roll, temp);
	tcflush(fd, TCIFLUSH);
	return 1;
}

int get_compass(double *ret_heading, double *ret_pitch, double *ret_roll, double *ret_temp, bool *goodData)
{
	if (read_compass() == 1)
		*goodData = true;
	else
		*goodData = false;
	*ret_heading = heading;
	*ret_pitch = pitch;
	*ret_roll = roll;
	*ret_temp = temp;
	return 0;
}

int display_compass()
{
	printf("Compass--- heading: %lf, roll: %lf, pitch: %lf, temp: %lf, goodData: %d\n", heading, pitch, roll, temp, goodData);
	return 0;
}



